#!/usr/bin/env python

import rospy, os
from task_planner.srv import *
from task_planner.msg import *
from roah_devices.srv import Bool
from roah_devices.msg import DevicesState
from std_srvs.srv import Empty

env = None
status_topic = None
i = 0

############# CLASS ###################
class Environment_class:
	
	def __init__(self):
		self.p_LightON = False
		self.p_BlindsON = False
		self.p_Presence = False
		self.p_AutoMode = True
		self.p_SunLight = False
		self.p_LightButton = False
		self.p_BlindsButton = False
	
	def update(self, p_LightON = None, p_BlindsON = None, p_Presence = None, p_AutoMode = None, p_SunLight = None,
				p_LightButton = None, p_BlindsButton = None):
		modified = False
		
		if p_LightON != None and p_LightON != self.p_LightON:
			self.p_LightON = p_LightON
			modified = True
		if p_BlindsON != None and p_BlindsON != self.p_BlindsON:
			self.p_BlindsON = p_BlindsON
			modified = True
		if p_Presence != None and p_Presence != self.p_Presence:
			self.p_Presence = p_Presence
			modified = True
		if p_AutoMode != None and p_AutoMode != self.p_AutoMode:
			self.p_AutoMode = p_AutoMode
			modified = True
		if p_SunLight != None and p_SunLight != self.p_SunLight:
			self.p_SunLight = p_SunLight
			modified = True
		if p_LightButton != None and p_LightButton != self.p_LightButton:
			self.p_LightButton = p_LightButton
			modified = True
		if p_BlindsButton != None and p_BlindsButton != self.p_BlindsButton:
			self.p_BlindsButton = p_BlindsButton
			modified = True
			
		if modified:
			publish_status()

################## SERVICE HANDLERS ################################
	
def handle_set_light(req):
	rospy.wait_for_service('/devices/switch_1/set')
	rospy.wait_for_service('/devices/switch_2/set')
	rospy.wait_for_service('/devices/switch_3/set')
	env.update(p_LightON = req.data)
	try:
		s1 = rospy.ServiceProxy('/devices/switch_1/set', Bool)
		s2 = rospy.ServiceProxy('/devices/switch_2/set', Bool)
		s3 = rospy.ServiceProxy('/devices/switch_3/set', Bool)
		s1(req.data)
		s2(req.data)
		s3(req.data)
	except rospy.ServiceException, e:
		env.update(p_LightON = not(req.data))
		print "error during setting the lights"
		
def handle_set_blinds(req):
	rospy.wait_for_service('/devices/blinds/max')
	rospy.wait_for_service('/devices/blinds/min')
	env.update(p_BlindsON = req.data)
	try:
		max = rospy.ServiceProxy('/devices/blinds/max', Empty)
		min = rospy.ServiceProxy('/devices/blinds/min', Empty)
		if req.data == True:
			max()
		else:
			min()
	except rospy.ServiceException, e:
		env.update(p_BlindsON = not(req.data))
		print "error during setting the p_BlindsON"

def handle_set_presence(req):
	env.update(p_Presence = req.data)
	return SetBoolResponse()
		
def handle_set_automatic_mode(req):
	env.update(p_AutoMode = req.data)
	
def handle_set_SunLight(req):
	env.update(p_SunLight = req.data)
	
def handle_set_LightButton(req):
	rospy.wait_for_service('/devices/switch_1/set')
	rospy.wait_for_service('/devices/switch_2/set')
	rospy.wait_for_service('/devices/switch_3/set')
	try:
		s1 = rospy.ServiceProxy('/devices/switch_1/set', Bool)
		s2 = rospy.ServiceProxy('/devices/switch_2/set', Bool)
		s3 = rospy.ServiceProxy('/devices/switch_3/set', Bool)
		s1(req.data)
		s2(req.data)
		s3(req.data)
	except rospy.ServiceException, e:
		print "error during setting the lights"
	#env.update(p_LightButton = req.data)
	
def handle_set_BlindsButton(req):
	rospy.wait_for_service('/devices/blinds/max')
	rospy.wait_for_service('/devices/blinds/min')
	try:
		max = rospy.ServiceProxy('/devices/blinds/max', Empty)
		min = rospy.ServiceProxy('/devices/blinds/min', Empty)
		if req.data == True:
			max()
		else:
			min()
	except rospy.ServiceException, e:
		print "error during setting the p_BlindsON"
	#env.update(p_BlindsButton = req.data)
	
	
######################## MESSAGES ###################################

def server_status_callback(status):
	if env.p_LightON != status.switch_1: #ignorati gli switch 2 e 3 perche' non avevo voglia
		env.update(p_LightON = status.switch_1, p_LightButton = True)
	if env.p_BlindsON != status.blinds:
		env.update(p_BlindsON = status.blinds, p_BlindsButton = True)
		
def environment_update_callback(status):
	p_LightON = None
	p_BlindsON = None
	p_AutoMode = None
	p_Presence = None
	p_SunLight = None
	p_LightButton = None
	p_BlindsButton = None
		
	if status.set_LightON:
		p_LightON = status.p_LightON
	if status.set_BlindsON:
		p_BlindsON = status.p_BlindsON
	if status.set_AutoMode:
		p_AutoMode = status.p_AutoMode
	if status.set_Presence:
		p_Presence = status.p_Presence
	if status.set_SunLight:
		p_SunLight = status.p_SunLight
	if status.set_LightButton:
		p_LightButton = status.p_LightButton
	if status.set_BlindsButton:
		p_BlindsButton = status.p_BlindsButton		
		
	env.update(p_LightON, p_BlindsON, p_Presence, p_AutoMode, p_SunLight, p_LightButton, p_BlindsButton)

	if status.set_LightON:
		rospy.wait_for_service('/devices/switch_1/set')
		rospy.wait_for_service('/devices/switch_2/set')
		rospy.wait_for_service('/devices/switch_3/set')
		try:
			s1 = rospy.ServiceProxy('/devices/switch_1/set', Bool)
			s2 = rospy.ServiceProxy('/devices/switch_2/set', Bool)
			s3 = rospy.ServiceProxy('/devices/switch_3/set', Bool)
			s1(status.p_LightON)
			s2(status.p_LightON)
			s3(status.p_LightON)
		except rospy.ServiceException, e:
			print "error during setting the lights"
	if status.set_BlindsON:
		rospy.wait_for_service('/devices/blinds/max')
		rospy.wait_for_service('/devices/blinds/min')
		try:
			max = rospy.ServiceProxy('/devices/blinds/max', Empty)
			min = rospy.ServiceProxy('/devices/blinds/min', Empty)
			if status.p_BlindsON == True:
				max()
			else:
				min()
		except rospy.ServiceException, e:
			print "error during setting the p_BlindsON"
	
def publish_status():
	global status_topic
	global i 
	i = i+1
	print "- Publish on topic environment/status: ", i
	s = Environment()
	s.p_LightON = env.p_LightON
	s.p_BlindsON = env.p_BlindsON
	s.p_Presence = env.p_Presence
	s.p_AutoMode = env.p_AutoMode
	s.p_SunLight = env.p_SunLight
	s.p_LightButton = env.p_LightButton
	s.p_BlindsButton = env.p_BlindsButton
	status_topic.publish(s)	

########################## MAIN #####################################		

def start_interface():
	global status_topic
	os.system("clear") 
	print " + Environment Interface: START \n"
	
	rospy.init_node('environment_interface')
	
	#services
	rospy.Service('/environment/set_LightON', SetBool, handle_set_light)
	rospy.Service('/environment/set_BlindsON', SetBool, handle_set_blinds)
	rospy.Service('/environment/set_Presence', SetBool, handle_set_presence)
	rospy.Service('/environment/set_AutoMode', SetBool, handle_set_automatic_mode)
	rospy.Service('/environment/set_SunLight', SetBool, handle_set_SunLight)
	rospy.Service('/environment/set_LightButton', SetBool, handle_set_LightButton)
	rospy.Service('/environment/set_BlindsButton', SetBool, handle_set_BlindsButton)
	
	#topics
	rospy.Subscriber("/devices/state", DevicesState, server_status_callback)
	rospy.Subscriber("/environment/modifications", EnvUpdate, environment_update_callback)
	status_topic = rospy.Publisher('/environment/status', Environment, latch = True)
	
	publish_status()
	rospy.spin()
	
if __name__ == '__main__':
	env = Environment_class()
	start_interface()
